#include <stdlib.h>

#include "../../util/macros.h"
#include "../../matrix/matrix.h"

#define INCX 5
#define INCY 5

void image_rad2quad(Matrix * Or, Matrix ** Quads) {

	double * or;
	double * quads;

	_verify(Or);
	_verify(Quads);

	or = Or->data;

	if (*Quads == NULL) (*Quads) = matrix_new(Or->rows, Or->columns);
	quads = (*Quads)->data;

	{
		long int i;
		double rad, deg;
		int quad;

		for (i = 0; i < Or->size; i++) {
			
			if( i >= 12486 ) {
				i = i;
			}
			rad = or[i];
			deg = rad2deg(rad);

			if (deg > 0 && deg <= 90) quad = 1;
			if (deg > 90 && deg <= 180) quad = 2;
			if (deg > 180 && deg <= 270) quad = 3;

			if (deg > 270 && deg <= 360) quad = 4;

			quads[i] = quad;

		}
	}

}

void image_get_chess_squares(Matrix * Corners, Matrix * Or, Matrix ** Squares, Matrix ** Quads) {

	double * corners;
	double * or;
	double * squares;
	double * quads;

	int sq_pred;

	_verify(Corners);
	_verify(Or);

	corners = Corners->data;
	or = Or->data;

	sq_pred = FLOOR(Corners->size / 4);

	if (Squares) if (*Squares == NULL) (*Squares) = matrix_new(sq_pred, 4);
	if (Squares) squares = (*Squares)->data;

	if (*Quads == NULL) (*Quads) = matrix_new(Or->rows, Or->columns);
	quads = (*Quads)->data;

	{
		int r, _r, i;
		int x, y;
		double rad, deg;
		int quad;

		for (r = 0, _r = 0; r < Corners->rows; r++, _r += Corners->columns) {

			x = corners[_r];
			y = corners[_r + 1];

			i = y * Or->columns + x;

			rad = or[i];
			deg = rad2deg(rad);

			if (deg >= 0 && deg < 90) quad = 1;
			if (deg >= 90 && deg < 180) quad = 2;
			if (deg >= 180 && deg < 270) quad = 3;
			if (deg >= 270 && deg <= 360) quad = 4;

			quads[i] = quad;
		}
	}



}


void image_find_chess_corners(void) {
	
	// 1. equalizes image
	// 2. cvCheckChessboard
	// 3. 
	
}